#include <stdlib.h>
#include "GenericTypeDefs.h"
#include "HardwareProfile.h"
#include "Delay.h"
#include "usb_config.h"
#include "USB/usb.h"
#include "USB/usb_host_generic.h"
#include "user.h"
#include "timer.h"
#include "tinymodules/tprintf.h"
#include "tinymodules/tsprintf.h"
#include "microbridge/Adb_pic.h"

#define PLL_96MHZ_OFF   0xFFFF
#define PLL_96MHZ_ON    0xF7FF

// for PIC24FJ64GB002
_CONFIG1(WDTPS_PS1 & FWPSA_PR32 & WINDIS_OFF & FWDTEN_OFF & ICS_PGx1 & GWRP_OFF & GCP_OFF & JTAGEN_OFF)
_CONFIG2(OSCIOFNC_OFF & POSCMOD_NONE & I2C1SEL_PRI & IOL1WAY_OFF & OSCIOFNC_ON & FCKSM_CSDCMD & FNOSC_FRCPLL & PLL96MHZ_ON & PLLDIV_DIV2 & IESO_OFF)
_CONFIG3(WPFP_WPFP0 & SOSCSEL_IO & WUTSEL_LEG & WPDIS_WPDIS & WPCFG_WPCFGDIS & WPEND_WPENDMEM)
_CONFIG4(DSWDTPS_DSWDTPS3 & DSWDTOSC_LPRC & RTCOSC_SOSC & DSBOREN_OFF & DSWDTEN_OFF)

extern BYTE     deviceAddress;  // Address of the device on the USB
void uart2_puts(char* );
void lprint(char* str,int len);
void initPeripheral(void);

static void setServoAngle(int ServoNo,int Angle);
static boolean datareceived = false;

const unsigned char command_code[] = "get\n";

int ledOnOff;
int applicationLaunchState = false;
int applicationLaunched = false;

int main ( void )
{
    Connection* c;
	int i;

    {
        CLKDIV = 0;
        unsigned int pll_startup_counter = 600;
        CLKDIVbits.PLLEN = 1;
        while(pll_startup_counter--);
    }

	// Configure U2RX - put on pin 17 (RP8)
	RPINR19bits.U2RXR = 8;
	// Configure U2TX - put on pin 16 (RP7)
	RPOR3bits.RP7R = 5;
	TRISAbits.TRISA0 = 1;		// PORT A0 -> INPUT
	TRISBbits.TRISB3 = 0;

	UART2Init();
    tprintf_SetCallBack(uart2_puts);

	initPeripheral();

    tprintf("\x1b[m" "**********************************************\r\n");
	tprintf(" PIC_microbridge_ledOnOff test program\r\n");
    tprintf(" SYSTEM STARTED Build:%s %s\r\n",__DATE__,__TIME__);
    tprintf("**********************************************\r\n");
    DelayMs(1000);

    ADB_init();			// initialize ADB module

    DelayMs(1000);

	applicationLaunchState = true;
	c = ADB_addConnection("shell:am start -n jp.yishii.microbridgesample/.MicrobridgeSampleActivity",false);

	while(applicationLaunched == false){
        ADB_poll(c);
		DelayMs(10);
	}
	ADB_delConnection(c);
	tprintf(" application launched\r\n");
    DelayMs(2000);

	c = ADB_addConnection("tcp:60101", false);

    while (1){
        ADB_poll(c);
		if(datareceived == true){
			tprintf("Led = %Xh\r\n",ledOnOff);
			if(ledOnOff == 0x31){
				LATBbits.LATB3 = 0;
			} else {
				LATBbits.LATB3 = 1;
			}
			ADB_write(c,4,command_code);
			datareceived = false;
		}
		DelayMs(1);
    }
    return 0;
}

void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
{
	int i;
/*
	ADB_CONNECT = 0,
	ADB_DISCONNECT,
	ADB_CONNECTION_OPEN,
	ADB_CONNECTION_CLOSE,
	ADB_CONNECTION_FAILED,
	ADB_CONNECTION_RECEIVE
 */
    switch(event){
    case ADB_CONNECTION_RECEIVE:
        tprintf("adbEventHandler : ADB_CONNECTION_RECEIVE len=%d\r\n",length);

		ledOnOff = data[0];
		datareceived = true;

	break;
    case ADB_CONNECTION_OPEN:
        tprintf("adbEventHandler : ADB_CONNECTION_OPEN\r\n");
		ledOnOff = 0x30;
		datareceived = true;
        break;
	case ADB_CONNECTION_FAILED:
		tprintf("adbEventHandler : ADB_CONNECTION_FAILED\r\n");
		break;
	case ADB_CONNECTION_CLOSE:
		tprintf("adbEventHandler : ADB_CONNECTION_CLOSE\r\n");
		if(applicationLaunchState == true){
			applicationLaunched = true;
			applicationLaunchState = false;
		}
		break;
    default:
        tprintf("[%s] event_id=%d\r\n",__func__,(int)event);
        break;
    }
}

void lprint(char* str,int len)
{
    char* buff;
    buff = malloc(len+1);
	if(buff == NULL){
		tprintf("insufficient memory area\r\n");
		return;
	}
    memcpy(buff,str,len);
    *(buff+len)=0x00;
    tprintf("\x1b[31m%s\x1b[m",buff);
    free(buff);
}

void uart2_puts(char* str)
{
    UART2PrintString(str);
}


void initPeripheral(void)
{
#define ServoScycle    30000-1
#define Angle_PlusMAX2 5000
#define Angle_SP	   3000-1
#define Angle_MinusMAX 2000-1
#define Angle_PlusMAX  4000-1

	TRISA = 0x00;
	TRISB = 0x00;

	TRISBbits.TRISB13 = 0;
	TRISBbits.TRISB14 = 0;
	TRISBbits.TRISB15 = 0;

	RPOR6bits.RP13R = 18;    // OC1
    RPOR7bits.RP14R = 19;    // OC2
    RPOR7bits.RP15R = 20;    // OC3

    T2CON = 0;
    T2CONbits.TCKPS = 0x01;    // プリスケーラ値 = 1/8。
    T2CONbits.TON = 1;

    PR2 = Angle_PlusMAX2;    // PlusMAX2 = 5000 => 2.5ms
    IFS0bits.T2IF = 0;

	// タイマ２のゼロクリアを待つ。
    while(!IFS0bits.T2IF);
	// セットされたT2IFのクリア。
    IFS0bits.T2IF = 0;
	// ここからがOC1のイニシアライズ。
    OC1CON1 = 0;
    OC1CON2 = 0;
	// 同期要因をOCモジュール自身にする。PWM出力モードではこの設定が必要。
    OC1CON2bits.SYNCSEL = 0x1f;
	// デューティ値をセットして、パルス幅を1.5ms（サーボの原点位置のパルス幅）
	// にする。サーボへ送るパルス幅の指定はOCxRレジスタへの値のセットでおこなう。
    OC1R = Angle_SP;
	// PWM周期の値をセットして、PWMの周期を15ms周期にする。
    OC1RS = ServoScycle;
	// 入力クロック源をタイマ２にする。
    OC1CON1bits.OCTSEL = 0x00;
	// モードをエッジ整列PWM出力モードにする。OC1のスタート。
    OC1CON1bits.OCM = 0x06;

	// 2.5ms待つ。
    while(!IFS0bits.T2IF);
    IFS0bits.T2IF = 0;
	// OC2のイニシアライズ。
    OC2CON1 = 0;
    OC2CON2 = 0;
    OC2CON2bits.SYNCSEL = 0x1f;
	// パルス幅を1.0ms（ -60°）にする。
    OC2R = Angle_MinusMAX;
    OC2RS = ServoScycle;
    OC2CON1bits.OCTSEL = 0x00;
    OC2CON1bits.OCM = 0x06;

	// 2.5ms待つ。
    while(!IFS0bits.T2IF);
    IFS0bits.T2IF = 0;
	// OC3のイニシアライズ。
    OC3CON1 = 0;
    OC3CON2 = 0;
    OC3CON2bits.SYNCSEL = 0x1f;
	// パルス幅を2.0ms（ +60°）にする。
    OC3R = Angle_PlusMAX;
    OC3RS = ServoScycle;
    OC3CON1bits.OCTSEL = 0x00;
    OC3CON1bits.OCM = 0x06;
}

static void setServoAngle(int ServoNo,int Angle)
{

	if(ServoNo == 0){
//		OC1R = 1275 + Angle * 2;
		OC1R = 1275 + Angle * 20;
		OC1RS = 30000-1;
		OC1CON1bits.OCTSEL = 0x00;
		OC1CON1bits.OCM = 0x06;
	} else {
		OC2R = 1275 + Angle * 20;
		OC2RS = 30000-1;
		OC2CON1bits.OCTSEL = 0x00;
		OC2CON1bits.OCM = 0x06;
	}
}
